
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_motors_class.c
  * @author     baiyang
  * @date       2023-1-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mb_motors_class.h"

#include <string.h>

#include <notify/notify.h>
#include <gcs_mavlink/gcs.h>
#include <srv_channel/srv_channel.h>
/*-----------------------------------macro------------------------------------*/
#define MB_MOTORS_SLEW_FILTER_CUTOFF 50.0f
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mb_motors_ctor(mb_motors_t motors, mb_motors_ops_t ops, uint16_t speed_hz)
{
    motors->ops = ops;

    motors->_speed_hz = speed_hz;
    derivative_filter7(&motors->_throttle_slew);
    motors->_spool_desired = MB_MOTOR_DESIRED_SHUT_DOWN;
    motors->_spool_state = MB_MOTOR_SHUT_DOWN;
    motors->_air_density_ratio = 1.0f;

    // setup throttle filtering
    lpf_set_cutoff1(&motors->_throttle_filter, 0.0f);
    lpf_reset(&motors->_throttle_filter, 0.0f);

    lpf_set_cutoff1(&motors->_throttle_slew_filter, MB_MOTORS_SLEW_FILTER_CUTOFF);
    lpf_reset(&motors->_throttle_slew_filter, 0.0f);

    // setup throttle slew detector
    derivative_filter7_reset(&motors->_throttle_slew);

    // init limit flags
    motors->limit.roll = true;
    motors->limit.pitch = true;
    motors->limit.yaw = true;
    motors->limit.throttle_lower = true;
    motors->limit.throttle_upper = true;
    motors->_thrust_boost = false;
    motors->_thrust_balanced = true;
}

/*
  map an internal motor mask to real motor mask, accounting for
  SERVOn_FUNCTION mappings, and allowing for multiple outputs per
  motor number
 */
uint32_t mb_motors_motor_mask_to_srv_channel_mask(mb_motors_const_t motors, uint32_t mask)
{
    uint32_t mask2 = 0;
    for (uint8_t i = 0; i < 32; i++) {
        uint32_t bit = 1UL << i;
        if (mask & bit) {
            Aux_servo_function_t function = srv_channels_get_motor_function(i);
            mask2 |= srv_channels_get_output_channel_mask(function);
        }
    }
    return mask2;
}

// returns a formatted string into buffer, e.g. "QUAD/X"
void mb_motors_get_frame_and_type_string(mb_motors_const_t motors, char *buffer, uint8_t buflen)
{
    const char *frame_str = mb_motors_get_frame_string(motors);
    const char *type_str = mb_motors_get_type_string(motors);
    if (type_str != NULL && strlen(type_str)) {
        rt_snprintf(buffer, buflen, "Frame: %s/%s", frame_str, type_str);
    } else {
        rt_snprintf(buffer, buflen, "Frame: %s", frame_str);
    }
}

void mb_motors_armed2(mb_motors_t motors, bool arm)
{
    if (motors->_armed != arm) {
        motors->_armed = arm;
        notify_flags.armed = arm;
        if (!arm) {
            mb_motors_save_params_on_disarm(motors);
        }
    }
};

// output_test_seq - spin a motor at the pwm value specified
//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void mb_motors_output_test_seq(mb_motors_t motors, uint8_t motor_seq, int16_t pwm)
{
    if (mb_motors_armed(motors) && motors->_interlock) {
        RT_ASSERT(motors->ops->_output_test_seq != RT_NULL);
        motors->ops->_output_test_seq(motors, motor_seq, pwm);
    }
}

void mb_motors_set_desired_spool_state(mb_motors_t motors, enum MotorDesiredSpoolState spool)
{
    if (motors->_armed || (spool == MB_MOTOR_DESIRED_SHUT_DOWN)) {
        motors->_spool_desired = spool;
    }
}

// set limit flag for pitch, roll and yaw
void mb_motors_set_limit_flag_pitch_roll_yaw(mb_motors_t motors, bool flag)
{
    motors->limit.roll = flag;
    motors->limit.pitch = flag;
    motors->limit.yaw = flag;
}

// pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle
void mb_motors_set_radio_passthrough(mb_motors_t motors, float roll_input, float pitch_input, float throttle_input, float yaw_input)
{
    motors->_roll_radio_passthrough = roll_input;
    motors->_pitch_radio_passthrough = pitch_input;
    motors->_throttle_radio_passthrough = throttle_input;
    motors->_yaw_radio_passthrough = yaw_input;
}

// returns true if the configured PWM type is digital and should have fixed endpoints
bool mb_motors_is_digital_pwm_type(mb_motors_const_t motors)
{
    switch ((enum mb_motor_pwm_type)motors->_pwm_type) {
        case MB_MOTOR_PWM_TYPE_DSHOT150:
        case MB_MOTOR_PWM_TYPE_DSHOT300:
        case MB_MOTOR_PWM_TYPE_DSHOT600:
        case MB_MOTOR_PWM_TYPE_DSHOT1200:
            return true;
        case MB_MOTOR_PWM_TYPE_NORMAL:
        case MB_MOTOR_PWM_TYPE_ONESHOT:
        case MB_MOTOR_PWM_TYPE_ONESHOT125:
        case MB_MOTOR_PWM_TYPE_BRUSHED:
        case MB_MOTOR_PWM_TYPE_PWM_RANGE:
            break;
    }
    return false;
}

void mb_motors_add_motor_num(mb_motors_t motors, int8_t motor_num)
{
    // ensure valid motor number is provided
    if (motor_num >= 0 && motor_num < MB_MOTORS_MAX_NUM_MOTORS) {
        Aux_servo_function_t function = srv_channels_get_motor_function(motor_num);
        srv_channels_set_aux_channel_default(function, motor_num);
    }
}


/// 
bool mb_motors_ArmingChecks(mb_motors_const_t motors, size_t buflen, char *buffer)
{
    if (!mb_motors_initialised_ok(motors)) {
        rt_snprintf(buffer, buflen, "Check frame class and type");
        return false;
    }

    return true;
}

// direct motor write
void mb_motors_RcWrite(mb_motors_t motors, uint8_t chan, uint16_t pwm)
{
    Aux_servo_function_t function = srv_channels_get_motor_function(chan);
    if ((1U<<chan) & motors->_motor_pwm_range_mask) {
        // note that PWM_MIN/MAX has been forced to 1000/2000
        srv_channels_set_output_scaled(function, pwm - 1000);
    } else {
        srv_channels_set_output_pwm(function, pwm);
    }
}

void mb_motors_RcWriteAngle(mb_motors_t motors, uint8_t chan, int16_t angle_cd)
{
    Aux_servo_function_t function = srv_channels_get_motor_function(chan);
    srv_channels_set_output_scaled(function, angle_cd);
}

void mb_motors_RcSetFreq(mb_motors_t motors, uint32_t motor_mask, uint16_t freq_hz)
{
    if (freq_hz > 50) {
        motors->_motor_fast_mask |= motor_mask;
    }

    const uint32_t mask = mb_motors_motor_mask_to_srv_channel_mask(motors, motor_mask);
    srv_hal_set_freq(mask, freq_hz);
    //hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type());

    switch ((enum mb_motor_pwm_type)motors->_pwm_type) {
    case MB_MOTOR_PWM_TYPE_ONESHOT:
        if (freq_hz > 50 && mask != 0) {
            srv_hal_set_output_mode(mask, MODE_PWM_ONESHOT);
        }
        break;
    case MB_MOTOR_PWM_TYPE_ONESHOT125:
        if (freq_hz > 50 && mask != 0) {
            srv_hal_set_output_mode(mask, MODE_PWM_ONESHOT125);
        }
        break;
    case MB_MOTOR_PWM_TYPE_BRUSHED:
        srv_hal_set_output_mode(mask, MODE_PWM_BRUSHED);
        break;
    case MB_MOTOR_PWM_TYPE_DSHOT150:
        srv_hal_set_output_mode(mask, MODE_PWM_DSHOT150);
        break;
    case MB_MOTOR_PWM_TYPE_DSHOT300:
        srv_hal_set_output_mode(mask, MODE_PWM_DSHOT300);
        break;
    case MB_MOTOR_PWM_TYPE_DSHOT600:
        srv_hal_set_output_mode(mask, MODE_PWM_DSHOT600);
        break;
    case MB_MOTOR_PWM_TYPE_DSHOT1200:
        srv_hal_set_output_mode(mask, MODE_PWM_DSHOT1200);
        break;
    case MB_MOTOR_PWM_TYPE_PWM_RANGE:
        /*
          this is a motor output type for multirotors which honours
          the SERVOn_MIN/MAX values per channel
         */
        motors->_motor_pwm_range_mask |= motor_mask;
        for (uint8_t i=0; i<16; i++) {
            if ((1U<<i) & motor_mask) {
                srv_channels_set_range(srv_channels_get_motor_function(i), 1000);
            }
        }
        srv_hal_set_output_mode(mask, MODE_PWM_NORMAL);
        break;
    default:
        srv_hal_set_output_mode(mask, MODE_PWM_NORMAL);
        break;
    }
}

/*------------------------------------test------------------------------------*/


